package pathfinder

import (
	"src/protoMsg"
	"src/zeus/linmath"
	"src/zeus/logger"
	"src/zeus/recast/detour"

	"github.com/arl/gogeo/f32/d3"
	assert "github.com/aurelien-rainone/assertgo"
)

const (
	MAX_POLYS int32 = 256
)

var AnchorPoint linmath.Vector3 = linmath.Vector3{
	Vector3: protoMsg.Vector3{X: -37800, Y: -5000, Z: -37790},
}

// var AnchorPoint linmath.Vector3 = linmath.NewVector3(-4800,-4800,-4800)
// 此处一定要用锚点去换算坐标，不能从客户端导出obj文件的时候做转换，不然会寻不到路，不知道为什么此处先打个tag
func RevetToUE4RealPosition(pos linmath.Vector3) linmath.Vector3 {
	pos.X += AnchorPoint.X
	pos.Y += AnchorPoint.Y
	pos.Z += AnchorPoint.Z
	tmp := pos.Y
	pos.Y = pos.Z
	pos.Z = tmp
	return pos
}

func NewNavgationPathFinder(navMesh *detour.NavMesh) IPathFinder {
	if navMesh == nil {
		return nil
	}
	strpath := make([]d3.Vec3, MAX_POLYS)
	for i := range strpath {
		strpath[i] = d3.NewVec3()
	}
	st, query := detour.NewNavMeshQuery(navMesh, 2048)
	if detour.StatusFailed(st) {
		return nil
	}
	return &NavmeshFinder{
		navMesh:           navMesh,
		navMeshQuery:      query,
		polyRefs:          make([]detour.PolyRef, MAX_POLYS),
		straightPath:      strpath,
		straightPathFlags: make([]uint8, MAX_POLYS),
		straightPathRefs:  make([]detour.PolyRef, MAX_POLYS),
		filter:            detour.NewStandardQueryFilter(),
		extents:           d3.NewVec3XYZ(4, 4, 4),
		AnchorPoint:       AnchorPoint,
	}
}

type NavmeshFinder struct {
	navMesh           *detour.NavMesh
	navMeshQuery      *detour.NavMeshQuery
	polyRefs          []detour.PolyRef
	straightPath      []d3.Vec3
	straightPathFlags []uint8
	straightPathRefs  []detour.PolyRef
	filter            *detour.StandardQueryFilter
	extents           d3.Vec3
	AnchorPoint       linmath.Vector3
}

func (this *NavmeshFinder) newVec3ByVector3(vec3 linmath.Vector3) d3.Vec3 {
	return d3.NewVec3XYZ((vec3.X+this.AnchorPoint.X)*-0.01, (vec3.Y+this.AnchorPoint.Y)*0.01, (vec3.Z+this.AnchorPoint.Z)*-0.01)
}

func (this *NavmeshFinder) newVec3ListToVector3List(path []d3.Vec3, len int) []linmath.Vector3 {
	vec3List := make([]linmath.Vector3, len, len)
	for i := 0; i < len; i++ {
		vec3List[i].X = path[i].X()*-100 - this.AnchorPoint.X
		vec3List[i].Y = path[i].Y()*100 - this.AnchorPoint.Y
		vec3List[i].Z = path[i].Z()*-100 - this.AnchorPoint.Z

		//fmt.Println("-----------------------------------------------------")
		//		//fmt.Println("recast -->",path[i])
		//		//fmt.Println("Server -->",vec3List[i])
		//		//fmt.Println("UE4 -->",RevetToUE4RealPosition(vec3List[i]))
	}
	return vec3List
}

func (this *NavmeshFinder) FindPath(stpos, edpos linmath.Vector3) ([]linmath.Vector3, bool) {
	if this.navMesh == nil || this.navMeshQuery == nil {
		return nil, false //fmt.Errorf("NavMesh or NavMeshQuery is null")
	}
	// navmesh_params, ok := params.(NavmeshParams)
	// if !ok {
	// 	log.Error("NavmeshFinder params not match")
	// 	return nil, false
	// }
	//curr := time.Now().UnixNano()/1e6
	var orgRef, dstRef detour.PolyRef // references of org/dst polygon refs
	//start.Y -= 88
	//end.Y	-= 88
	var nearestOrg d3.Vec3
	var nearestDst d3.Vec3
	var st detour.Status
	var org d3.Vec3 = this.newVec3ByVector3(stpos)
	var dst d3.Vec3 = this.newVec3ByVector3(edpos)

	st, orgRef, nearestOrg = this.navMeshQuery.FindNearestPoly(org, this.extents, this.filter)
	st, dstRef, nearestDst = this.navMeshQuery.FindNearestPoly(dst, this.extents, this.filter)
	//fmt.Println("------>>",start,org,nearestOrg,RevetToUE4RealPosition(start)," ; ",end,dst,nearestDst,RevetToUE4RealPosition(end))
	if detour.StatusFailed(st) {
		return nil, false //fmt.Errorf("FindNearestPoly failed with %v\n", st)
	} else if orgRef == 0 {
		//fmt.Println("------>>",navmesh_params.start,org,nearestOrg,RevetToUE4RealPosition(navmesh_params.start)," ; ",navmesh_params.end,dst,nearestDst,RevetToUE4RealPosition(navmesh_params.end))
		return nil, false //fmt.Errorf("org doesn't intersect any polygons")
	}
	assert.True(this.navMesh.IsValidPolyRef(orgRef), "%d is not a valid poly ref")
	//copy(org, nearestOrg)

	if detour.StatusFailed(st) {
		return nil, false //fmt.Errorf("FindNearestPoly failed with %v\n", st)
	} else if dstRef == 0 {
		//fmt.Println("------>>",navmesh_params.start,org,nearestOrg,RevetToUE4RealPosition(navmesh_params.start)," ; ",navmesh_params.end,dst,nearestDst,RevetToUE4RealPosition(navmesh_params.end))
		return nil, false //fmt.Errorf("dst doesn't intersect any polygons")
	}
	assert.True(this.navMesh.IsValidPolyRef(dstRef), "%d is not a valid poly ref")
	//copy(dst, nearestDst)

	var pathCount int
	pathCount, st = this.navMeshQuery.FindPath(orgRef, dstRef, nearestOrg, nearestDst, this.filter, this.polyRefs)
	if detour.StatusFailed(st) {
		return nil, false //fmt.Errorf("query.FindPath failed with %v\n", st)
	}
	//var dstT d3.Vec3 = dst
	//if (this.polyRefs[pathCount-1] != dstRef){
	//	posOverPoly := false
	//	this.navMeshQuery.ClosestPointOnPoly(this.polyRefs[pathCount-1], dst, dstT, &posOverPoly);
	//}

	var straightPathCount int
	straightPathCount, st = this.navMeshQuery.FindStraightPath(org, dst, this.polyRefs[:pathCount], this.straightPath, this.straightPathFlags, this.straightPathRefs, 0)
	if detour.StatusFailed(st) {
		return nil, false //fmt.Errorf("query.FindStraightPath failed with 0x%x\n", st)
	}
	pathList := this.newVec3ListToVector3List(this.straightPath[:straightPathCount], straightPathCount)
	//if time.Now().UnixNano()/1e6 - curr > 1{
	//	fmt.Println("MoveToPos -----------------------cost time = " ,time.Now().UnixNano()/1e6 - curr)
	//	fmt.Println("------>>",navmesh_params.start,org,nearestOrg,RevetToUE4RealPosition(navmesh_params.start)," ; ",navmesh_params.end,dst,nearestDst,RevetToUE4RealPosition(navmesh_params.end))
	//}
	//fmt.Println("->  path = ",org,dst,path[:pathCount],straightPath[:straightPathCount])
	return pathList, true
}

// GetNear 获取某个点附近的合法位置
// 参数：
//
//	params 检查点 *linmath.Vector3 类型
//
// 返回值：
//
//	pos 路点，注意第一个点就是输入的起始点
//	result 是否能成功
func (this *NavmeshFinder) GetNear(srcPos *linmath.Vector3) (*linmath.Vector3, bool) {

	return nil, false
}

func (this *NavmeshFinder) CheckWall(srcPos, dstPos linmath.Vector3) (path []linmath.Vector3, bHaveWall bool) {
	bHaveWall = false
	return
}

// Raycast 射线检测
func (this *NavmeshFinder) Raycast(params interface{}) (*linmath.Vector3, bool) {
	return nil, false
}

func (this *NavmeshFinder) RaycastEx(srcPos, dstPos linmath.Vector3) (*linmath.Vector3, bool) {
	return nil, false
}

// 动态阻挡相关
// 增加可旋转的Box阻挡
func (this *NavmeshFinder) CreateObstacleOBBBox(doorName string, centPos, halfPos linmath.Vector3, YawDegree int32) {
}

// 增加圆柱体阻挡
func (this *NavmeshFinder) CreateObstacleCylinder(doorName string, centPos linmath.Vector3, radius, height int32) {
}

// 清理掉某个门
func (this *NavmeshFinder) RemoveObstacle(doorName string) {}

// 清理全部的门
func (this *NavmeshFinder) RemoveAllObstacle() {

}

// 更新阻挡信息
func (this *NavmeshFinder) UpdateObstacle() {

}

func (this *NavmeshFinder) Destroy() {

}

func (this *NavmeshFinder) MeshCheckRushPath(srcPos, stpos linmath.Vector3) linmath.Vector3 {
	if tmpPos, ok := this.RaycastEx(srcPos, stpos); !ok {
		logger.Debug(logger.LogKey_MoveSys, " [FindCollection] MeshCheckRushPath 未能检测到墙直接通过 ", stpos)
		return stpos
	} else {
		if tmpPos.IsEqualXZ(srcPos) {
			if path, ok := this.FindPath(srcPos, stpos); ok {
				if len(path) == 2 {
					*tmpPos = path[1]
					logger.Debug(logger.LogKey_MoveSys, " [FindCollection] MeshCheckRushPath 检测到墙 再次尝试走到墙根 ", path[1])
				}
			}
		} else {
			logger.Debug(logger.LogKey_MoveSys, " [FindCollection] MeshCheckRushPath 检测到墙 ", tmpPos)
		}
		stpos = *tmpPos
		return stpos
	}
}
